#! /usr/bin/python3
import actionlib.simple_action_client
import rospy
import actionlib

from control_msgs.msg import FollowJointTrajectoryAction,FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory,JointTrajectoryPoint

class TrajectoryDemo():
    def __init__(self):
        rospy.init_node('trajectory_demo')

        reset = rospy.get_param('~reset',False)

        saber_joints=['joint1',
                    'joint2',
                    'joint3',
                    'joint4',
                    'joint5',
                    'joint6']

        if reset:
            saber_goal=[0,0,0,0,0,0]

        else:
            saber_goal=[-0.3,-1.0,0.5,0.8,1.0,-0.7]

        rospy.loginfo('waiting for arm trajectory controller...')
        saber_client=actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory',FollowJointTrajectoryAction)
        saber_client.wait_for_server()
        rospy.loginfo('...connected.')

        saber_trajectory=JointTrajectory()
        saber_trajectory.joint_names=saber_joints
        saber_trajectory.points.append(JointTrajectoryPoint())
        saber_trajectory.points[0].positions=saber_goal
        saber_trajectory.points[0].velocities=[0.0 for i in saber_joints]
        saber_trajectory.points[0].accelerations=[0.0 for i in saber_joints]
        saber_trajectory.points[0].time_from_start=rospy.Duration(3.0)

        rospy.loginfo('Moving saber to goal position...')

        saber_goal=FollowJointTrajectoryGoal()
        saber_goal.trajectory=saber_trajectory
        saber_goal.goal_time_tolerance=rospy.Duration(0.0)
        saber_client.send_goal(saber_goal)
        saber_client.wait_for_result(rospy.Duration(5.0))

if __name__ == "__main__":
    try:
        TrajectoryDemo()
    except rospy.ROSInternalException:
        pass